/// 相机内参计算, 按照张正友写的方法 （TODO: 结果个不对）
/// f u0, v0 计算
/// 需要两个平面才能计算出来
///
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include <memath/meTransform.h>
#include <memath/MeEigenMath.h>

using namespace std;

// 数据来自于一影采集的2D图像，图像大小为2064*2064
// 二维标尺有上下两个面，采集两次得到4个面的数据
const static double s_objPoints[] = {
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane2
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
    //55.06, 5.75, 2,
    // plane3
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane4
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
};

const static double s_imagePoints[] = {
    558.791 ,1481.58,
    175.822 ,901.062,
    754.578 ,534.524,
    1130.44 ,1109.1,
    585.582 ,1352.91,
    // plane2
    1317.02 ,573.621,
    386.046 ,264.715,
    166.893 ,1322.2,
    1147.49 ,1423.43,
    1401.28 ,520.14,
    //1152.33 ,519.293,

    // plane3
    1290.68 ,1513.81,
    783.605 ,1045.01,
    1254.05 ,542.663,
    1757.41 ,1011.03,
    1286.35 ,1381.44,
    // plane4
    1782.11 ,544.629,
    801.412 ,479.553,
    845.876 ,1559.89,
    1818.47 ,1418.17,
    1851.84 ,470.476,
    //1607.66 ,532.807,
};

/// 根据角度生成测试数据, 输入的为弧度
void createIdealData(double rx, double ry, double rz, vector<Eigen::Vector3d> &p3ds, vector<Eigen::Vector2d> &p2ds)
{
    // 生成数据EulerZYX
    Matrix4d m = MeTransform::rotateZ(rz*ME_DEGREE2RAD)
            *MeTransform::rotateY(ry*ME_DEGREE2RAD)
            *MeTransform::rotateX(rx*ME_DEGREE2RAD);

    m(0, 3) = 1;
    m(1, 3) = 2;
    m(2, 3) = 300;
    double f = 500;
    double u = 400, v=400;

    vector<Eigen::Vector3d> p_I = {
        {10, 10, 60},
        {10, 10, 20},
        {20, 10, 10},
        {20, 0, 0},
        {70, 70, 180},
        {60, 120, 300},
        {0, 0, 300},
        {300, 0, 0},
        {0, 300, 0},
        {0, 0, 0},
    };

    vector<Eigen::Vector3d> p_O;
    for(uint i=0; i< p_I.size(); i++) {
        Eigen::Vector3d pO = MeEigenMath::multiplyVector3d(m, p_I[i]);
        pO[0] = pO[0]*f/pO[2] + u;
        pO[1] = pO[1]*f/pO[2] + v;
        pO[2] = 1;
        p_O.push_back(pO);
    }

    p3ds = p_I;
}


void createRuler2DRealData(vector<cv::Point2f> &objPts, vector<cv::Point2f> &imagePts, const int nPlane = 0)
{
    int sIndex = 5*nPlane;
    int nPts = 5;

    const double *p1 = s_objPoints+3*sIndex;
    const double *p2 = s_imagePoints+2*sIndex;
    objPts.clear();
    imagePts.clear();
    for (int i=0; i < nPts; i++) {
        objPts.push_back(cv::Point2f(p1[i*3+0], p1[i*3+1]));
        imagePts.push_back(cv::Point2f(p2[i*2+0], p2[i*2+1]));
    }
}

/// 6个参数无法计算出
void calIntrinsicParams(cv::Mat *H, int nPlane)
{
    // Vb = 0
    // b = [B11 B12 B22 B13 B23 B33]^T
    cv::Mat B = cv::Mat(6, 1, CV_64F);
    cv::Mat V = cv::Mat(nPlane*2+1, 6, CV_64F);
    for (uint i=0; i<nPlane; i++) {
        double h1 = H[i].at<double>(0, 0);
        double h2 = H[i].at<double>(1, 0);
        double h3 = H[i].at<double>(2, 0);
        double v1 = H[i].at<double>(0, 1);
        double v2 = H[i].at<double>(1, 1);
        double v3 = H[i].at<double>(2, 1);

        double *p1 = V.ptr<double>(i*2+0);
        p1[0] = h1*v1;
        p1[1] = h1*v2+h2*v1;
        p1[2] = h2*v2;
        p1[3] = h3*v1+h1*v3;
        p1[4] = h3*v2 + h2*v3;
        p1[5] = h3*v3;
        double *p2 = V.ptr<double>(i*2+1);
        p2[0] = h1*h1 - v1*v1;
        p2[1] = h1*h2+h2*h1 - (v1*v2+v2*v1);
        p2[2] = h2*h2-v2*v2;
        p2[3] = h3*h1+h1*h3-(v3*v1+v1*v3);
        p2[4] = h3*h2 + h2*h3-(v3*v2 + v2*v3);
        p2[5] = h3*h3 - v3*v3;
    }
    // B12 = 0
    double *p1 = V.ptr<double>(nPlane*2);
    p1[0] = 0;
    p1[1] = 1;
    p1[2] = 0;
    p1[3] = 0;
    p1[4] = 0;
    p1[5] = 0;

    cout << "V: \n" << V << endl;
    cv::SVD::solveZ(V, B);
    cout << "B: \n" << B << endl;

    double *b = B.ptr<double>();
    double fx = std::sqrt(fabs(1./b[0]));
    double fy = std::sqrt(fabs(1./b[2]));

    cout << "f: \n" << fx << ", " << fy << endl;
}

/// r = 0
void calIntrinsicParams2(cv::Mat *H, int nPlane)
{
    // Vb = 0
    // b = [B11 B22 B13 B23 B33]^T
    cv::Mat B = cv::Mat(5, 1, CV_64F);
    cv::Mat V = cv::Mat(nPlane*2, 5, CV_64F);
    for (uint i=0; i<nPlane; i++) {
        // H = [h, v, t]
        double h1 = H[i].at<double>(0, 0);
        double h2 = H[i].at<double>(1, 0);
        double h3 = H[i].at<double>(2, 0);
        double v1 = H[i].at<double>(0, 1);
        double v2 = H[i].at<double>(1, 1);
        double v3 = H[i].at<double>(2, 1);

        double *p1 = V.ptr<double>(i*2+0);
        p1[0] = h1*v1;
        p1[1] = h2*v2;
        p1[2] = h3*v1+h1*v3;
        p1[3] = h3*v2 + h2*v3;
        p1[4] = h3*v3;
        double *p2 = V.ptr<double>(i*2+1);
        p2[0] = h1*h1 - v1*v1;
        p2[1] = h2*h2-v2*v2;
        p2[2] = h3*h1+h1*h3-(v3*v1+v1*v3);
        p2[3] = h3*h2 + h2*h3-(v3*v2 + v2*v3);
        p2[4] = h3*h3 - v3*v3;
    }

    cout << "V: \n" << V << endl;
    cv::SVD::solveZ(V, B);
    cout << "B: \n" << B << endl;

    double *b = B.ptr<double>();
    double u0 = b[3]/b[0];
    double v0 = -b[4]/b[1];
    double fx = std::sqrt(fabs(1./b[0]));
    double fy = std::sqrt(fabs(1./b[1]));

    cout << "u0: " << u0 << ", v0:" << v0 << endl;
    cout << "f: \n" << fx << ", " << fy << endl;
}

/// fx = fy
void calIntrinsicParams3(cv::Mat *H, int nPlane)
{
    // Vb = 0
    // b = [B11 B13 B23 B33]^T
    cv::Mat B = cv::Mat(4, 1, CV_64F);
    cv::Mat V = cv::Mat(nPlane*2, 4, CV_64F, 1);

    for (uint i=0; i<nPlane; i++) {
        // H = [h, v, t]
        double h1 = H[i].at<double>(0, 0);
        double h2 = H[i].at<double>(1, 0);
        double h3 = H[i].at<double>(2, 0);
        double v1 = H[i].at<double>(0, 1);
        double v2 = H[i].at<double>(1, 1);
        double v3 = H[i].at<double>(2, 1);

        double *p1 = V.ptr<double>(i*2+0);
        p1[0] = h1*v1 + h2*v2;
        p1[1] = h3*v1+h1*v3;
        p1[2] = h3*v2 + h2*v3;
        p1[3] = h3*v3;
        cout << "V: \n" << V << endl;
        double *p2 = V.ptr<double>(i*2+1);
        p2[0] = h1*h1 + h2*h2 - (v1*v1 +v2*v2);
        p2[1] = h3*h1+h1*h3 - (v3*v1+v1*v3);
        p2[2] = h3*h2 + h2*h3-(v3*v2 + v2*v3);
        p2[3] = h3*h3 - v3*v3;
        cout << "V: \n" << V << endl;
    }

    cout << "V: \n" << V << endl;
    cv::SVD::solveZ(V, B);
    cout << "B: \n" << B << endl;

    double *b = B.ptr<double>();
    double u0 = b[1]/b[0];
    double v0 = -b[2]/b[0];
    double fx = std::sqrt(fabs(1./b[0]));
    double fy = std::sqrt(fabs(1./b[0]));

    cout << "u0: " << u0 << ", v0:" << v0 << endl;
    cout << "f: \n" << fx << ", " << fy << endl;
}

/// TODO: 无法求出内参
int main(int argc, char *argv[])
{
    const int nPlane = 2;
    // 根据给定数据计算单应矩阵H
    vector<vector<cv::Point2f> > imgPts(nPlane);
    vector<vector<cv::Point2f> > objPts(nPlane); // 为三维坐标z=0的值
    cv::Mat H[nPlane];
    for (uint i=0; i<nPlane; i++) {
        createRuler2DRealData(objPts[i], imgPts[i], i);
        // cout << "objPts: " << objPts[i] << endl;
        // cout << "imgPts: " << imgPts[i] << endl;
        H[i] = cv::findHomography(objPts[i], imgPts[i]);
        cout << "H:\n" << H[i] << endl;

    }
    // 根据H计算B的值 Vb = 0
    calIntrinsicParams3(H, nPlane);

    return 0;
}



